#include "spider_leg.h"

#include "arm_math.h"
#include "math.h"

//--------------本地函数声明------------------
static float get_points_distance(float x1, float y1, float z1, float x2,
                                 float y2, float z2);

//---------------函数实现------------------
void Leg_set_pos(Leg *leg_1, Leg *leg_2, Leg *leg_3, float yaw, float pitch,
                 float roll, _Bool deg_mode) {
    // 腿长
    float L1 = leg_1->leg_length;
    float L2 = leg_2->leg_length;
    float L3 = leg_3->leg_length;

    if (deg_mode) {
        // 将角度转换为弧度
        yaw = yaw * PI / 180.0f;
        pitch = pitch * PI / 180.0f;
        roll = roll * PI / 180.0f;
    }

    float forward_kinematics_result[12] = {0};  // 正向运动学结果
    Leg_forward_kinematics(yaw, pitch, roll, L1, L2, L3,
                           forward_kinematics_result);

    // 设置腿的目标位置
    leg_1->start_x = forward_kinematics_result[0];
    leg_1->start_y = forward_kinematics_result[1];
    leg_1->start_z = forward_kinematics_result[2];
    leg_1->end_x = forward_kinematics_result[3];
    leg_1->end_y = forward_kinematics_result[4];
    leg_1->end_z = forward_kinematics_result[5];

    leg_2->start_x = forward_kinematics_result[3];
    leg_2->start_y = forward_kinematics_result[4];
    leg_2->start_z = forward_kinematics_result[5];
    leg_2->end_x = forward_kinematics_result[6];
    leg_2->end_y = forward_kinematics_result[7];
    leg_2->end_z = forward_kinematics_result[8];

    leg_3->start_x = forward_kinematics_result[6];
    leg_3->start_y = forward_kinematics_result[7];
    leg_3->start_z = forward_kinematics_result[8];
    leg_3->end_x = forward_kinematics_result[9];
    leg_3->end_y = forward_kinematics_result[10];
    leg_3->end_z = forward_kinematics_result[11];
}

void Leg_forward_kinematics(float theta1, float theta2, float theta3, float L1,
                            float L2, float L3, float *output_array) {
    // 计算各个关键点坐标
    float x0 = 0, y0 = 0, z0 = 0;  // 基座

    // 第1节手臂末端位置（yaw）
    float x1 = L1 * arm_cos_f32(theta1);
    float y1 = L1 * arm_sin_f32(theta1);
    float z1 = 0;

    // 第2节手臂末端位置（pitch）
    float x2 = x1 + L2 * arm_cos_f32(theta2) * arm_cos_f32(theta1);
    float y2 = y1 + L2 * arm_cos_f32(theta2) * arm_sin_f32(theta1);
    float z2 = z1 + L2 * arm_sin_f32(theta2);

    // 第3节手臂末端位置（pitch）
    float x3 = x2 + L3 * arm_cos_f32(theta2 + theta3) * arm_cos_f32(theta1);
    float y3 = y2 + L3 * arm_cos_f32(theta2 + theta3) * arm_sin_f32(theta1);
    float z3 = z2 + L3 * arm_sin_f32(theta2 + theta3);

    // 输出结果
    output_array[0] = x0;  // 基座坐标
    output_array[1] = y0;
    output_array[2] = z0;

    output_array[3] = x1;  // 第一节手臂末端坐标
    output_array[4] = y1;
    output_array[5] = z1;

    output_array[6] = x2;  // 第二节手臂末端坐标
    output_array[7] = y2;
    output_array[8] = z2;

    output_array[9] = x3;  // 第三节手臂末端坐标
    output_array[10] = y3;
    output_array[11] = z3;
}

Leg_IK_Status Leg_inverse_kinematics(float x, float y, float z, float L1,
                                     float L2, float L3, float *output_array) {
    // 计算 yaw，绕 z 轴旋转
    float yaw = atan2f(y, x);

    // theta3 容易得到，先计算
    // 接下来要计算机械臂一号关节和二号关节之间的夹角
    // 故先计算一号关节的坐标
    float x1 = L1 * arm_cos_f32(yaw);
    float y1 = L1 * arm_sin_f32(yaw);
    float z1 = 0;

    // 计算一号关节和三号关节之间的距离
    float r = get_points_distance(x, y, z, x1, y1, z1);
    // 计算 D_cos
    float D_cos = (L2 * L2 + L3 * L3 - r * r) / (2 * L2 * L3);
    // 判断 D_cos 是否在范围内
    if (D_cos < -1 || D_cos > 1) {
        output_array[0] = -5;
        output_array[1] = -5;
        output_array[2] = -5;
        return D_COS_OUT_OF_RANGE;
    }

    float theta3 = PI - acosf(D_cos);

    // 计算 theta2
    float R = get_points_distance(x, y, z, 0, 0, 0);
    float D_1_cos = (L1 * L1 + r * r - R * R) / (2 * L1 * r);
    if (D_1_cos < -1 || D_1_cos > 1) {
        output_array[0] = -5;
        output_array[1] = -5;
        output_array[2] = -5;
        return D_1_COS_OUT_OF_RANGE;
    }

    float D_1 = acosf(D_1_cos);
    float D_2_cos = (L2 * L2 + r * r - L3 * L3) / (2 * L2 * r);
    if (D_2_cos < -1 || D_2_cos > 1) {
        output_array[0] = -5;
        output_array[1] = -5;
        output_array[2] = -5;
        return D_2_COS_OUT_OF_RANGE;
    }

    float D_2 = acosf(D_2_cos);
    float theta2 = PI - (D_1 + D_2);

    // 输出结果
    output_array[0] = yaw;      // 关节1的角度
    output_array[1] = -theta2;  // 关节2的角度 现在为elbow up，所以要取反
    output_array[2] = -theta3;  // 关节3的角度 现在为elbow up，所以要取反

    return LEG_IK_OK;
}

//---------------本地函数实现------------------
static float get_points_distance(float x1, float y1, float z1, float x2,
                                 float y2, float z2) {
    // 计算两点之间的距离
    float output;
    float temp = powf(x2 - x1, 2) + powf(y2 - y1, 2) + powf(z2 - z1, 2);

    arm_sqrt_f32(temp, &output);
    return output;
}